function [hMap,flagkxcar,zmin3d,zmax3d,Nf]  = contiObsMap_dymap_G(obs1,dx,dy,rsafe,zmin,zmax,nz,obsnf,Ahmap,pilerange,selectpile)
tic;
obsnf=ceil(obs1(end,1));
obs=obs1(1:2:obsnf,:);
n1=nz(1);n2=nz(2);zmin=zmin(:,1:2);zmax=zmax(:,1:2);
gMap = (rsafe+4)*ones(n1*n2,1);
d = [n1,n2];
deltaX = (zmax(1)-zmin(1))/d(1);deltaY = (zmax(2)-zmin(2))/d(2);
for k = 1:size(obs,1)
    xK = obs(k,1);yK = obs(k,2);
    minIX = ((xK - dx)-zmin(1))/deltaX+0.5;
    maxIX = ((xK + dx)-zmin(1))/deltaX+0.5;
    minIY = ((yK - dy)-zmin(2))/deltaY+0.5;
    maxIY = ((yK + dy)-zmin(2))/deltaY+0.5;
    iX_ = max(floor(minIX),1):1:min(ceil(maxIX),d(1));
    iY_ = max(floor(minIY),1):1:min(ceil(maxIY),d(2));
    iX_t=repmat(iX_,numel(iY_),1); iY_t=repmat(iY_',numel(iX_),1);
    iX3 = iX_t(:);iY3 = iY_t(:);
    iXY_ = iX3 + (iY3-1)*d(1);
    obsk=obs(k,:);
    dd = (zmin-obsk);
    der=[ones(size(iX3,1),1)*dd(1),ones(size(iX3,1),1)*dd(2)]+[(iX3-0.5)*deltaX,(iY3-0.5)*deltaY];
    normer=sqrt(sum(der.^2,2));
    i1=normer<gMap(iXY_);
    gMap(iXY_(i1)) = normer(i1);
end
drmax=0.5;
nz3d=nz;
Nf=nz3d(1)*nz3d(2)*nz3d(3);
hMap = zeros(400*400*50,1,'int8');
zmin3d=[zmin,-pi];zmax3d=[zmax,pi];
pilemin=pilerange(1,:);pilemax=pilerange(2,:);
[ZZ,idzmap1]=fsg3d1(nz3d,zmin3d,zmax3d,pilemin,pilemax);
Z0=zeros(3050000,3);idzmap=ones(3050000,1);
Z0(1:size(ZZ,1),:)=ZZ(:,1:3);idzmap(1:size(ZZ,1),:)=idzmap1(:);
dis2obs=mindisobs(Z0,gMap,nz3d,zmin3d,zmax3d);
hmap=10*max(0,rsafe-dis2obs)/rsafe+2*max(0,rsafe+drmax-dis2obs)/drmax...
    +1*max(0,rsafe+2*drmax-dis2obs)/drmax...
    +0.25*max(0,rsafe+3*drmax-dis2obs)/drmax...
    +0.125*max(0,rsafe+4*drmax-dis2obs)/drmax;
hmap1=zeros(size(Z0,1),1);
hmap2=hmap1;
hmap1(:)=hmap(:,1)/Ahmap(:,1)*8;
hmap2(:)=hmap(:,2)/Ahmap(:,2)*8;
hmap3=max(hmap1,hmap2);
hMap(idzmap)=ceil(hmap3);
flagkxcar=ones(400*400*50,1,'int8');
flagkxcardy=dis2obs(:,1)>rsafe&dis2obs(:,2)>rsafe;
flagkxcar(idzmap)=flagkxcardy(:);
end
function [zMap,idMap]=fsg3d1(d,zmin,zmax,pilemin,pilemax)
deltaX = (zmax(1)-zmin(1))/d(1);deltaY = (zmax(2)-zmin(2))/d(2);deltaTh=(zmax(3)-zmin(3))/d(3);
xmin=pilemin(:,1);ymin=pilemin(:,2);xn=ceil((pilemax(:,1)-pilemin(:,1))/deltaX);yn=ceil((pilemax(:,2)-pilemin(:,2))/deltaY);
x1=xmin+(1:xn)*deltaX;y1=ymin+(1:yn)*deltaY;
ith=1:d(3);
tth1=zmin(3)+(ith-0.5)*deltaTh;
[tth,y,x]=meshgrid(tth1,y1,x1);
zMap=zeros(xn*yn*d(3),3);
zMap(:,1)=x(:);
zMap(:,2)=y(:);
zMap(:,3)=tth(:);
idMap=getindex3d1(zMap(:,1),zMap(:,2),zMap(:,3),d,zmin,zmax);
end
function dis2obs=mindisobs(Z0,gMap,nz3d,zmin,zmax)
x=Z0(:,1);y=Z0(:,2);th=Z0(:,3);
dx=(zmax(1)-zmin(1))/(nz3d(1));
dy=(zmax(2)-zmin(2))/(nz3d(2));
vcos=cos(th);vsin=sin(th);
lf=3.5;lr=3;
K=3;
N=size(Z0,1);
dis2obs=zeros(N,2);
%初始化前后车距障碍物的大小矩阵
rf=100*ones(N,1);
rb=rf;
%计算前车线距离障碍物的最短距离
for k=0:K
    %计算当前位置坐标
    xk=x+k/K*lf*vcos;
    yk=y+k/K*lf*vsin;
    %计算当前位置所对应的栅格位置
    ix=ceil((xk-zmin(1))/dx);
    iy=ceil((yk-zmin(2))/dy);
    i2d=ix+(iy-1)*nz3d(1);    
    %找出边界内的栅格位置索引
    ir1=(ix>=1)&(ix<=nz3d(1))&(iy>=1)&(iy<=nz3d(2));
    i2d(~ir1)=1;rf=min(rf,gMap(i2d));rf(~ir1)=0;
    %查表得出边界内的栅格距离障碍物的最短距离
    % rf(ir1)=min(rf(ir1),gMap(i2d(ir1)));   
end
%计算后车线距离障碍物的最短距离
for k=1:K
    xk=x-k/K*lr*vcos;
    yk=y-k/K*lr*vsin;
    ix=ceil((xk-zmin(1))/dx);
    iy=ceil((yk-zmin(2))/dy);
    i2d=ix+(iy-1)*nz3d(1);
    %找出边界内的栅格位置索引
    ir2=(ix>=1)&(ix<=nz3d(1))&(iy>=1)&(iy<=nz3d(2));
    %查表得出边界内的栅格距离障碍物的最短距离
    %rb(ir2)=min(rb(ir2),gMap(i2d(ir2)));
    i2d(~ir2)=1;rb=min(rb,gMap(i2d));rb(~ir2)=0;
end
%和并得到三维栅格前后车距离障碍物的最短距离
dis2obs(:)=[rf,rb];
end
function [k,ir]=getindex3d1(xx,yy,th,nz,zmin,zmax)
dz=(zmax-zmin)./nz;
tth1=mod(th,2*pi);tth1(tth1>pi)=tth1(tth1>pi)-2*pi;
tth=tth1-0.001*sign(tth1)+0.0001;
id1=ceil((xx-zmin(1))/dz(1));
id2=ceil((yy-zmin(2))/dz(2));
id3=ceil((tth-zmin(3))/dz(3));
ir=(id1>=1)&(id1<=nz(1))&(id2>=1)&(id2<=nz(2))&(id3>=1)&(id3<=nz(3));
k=id1+(id2-1)*nz(1)+(id3-1)*nz(1)*nz(2);
k(~ir)=1;
end